#include "Control.hpp"
#include <cmath>
#include <cstring>

// 预计算得到的伪逆矩阵 (MATLAB命令: pinv(B))
// B矩阵为:
// [0,0,0,0,1,-1,0,0;     % Fx
//  0,0,0,0,0,0,1,-1;     % Fy
//  0.3182,-0.3182,0.3182,-0.3182,0,0,0,0]  % Mz (l_arm=sqrt(2)*0.225=0.3182)
const float B_pinv[8][3] = {
    // 对角电机 (贡献Mz)
    {0.000000f, 0.000000f, 0.785674f},  // 电机1: 对角电机1
    {0.000000f, 0.000000f, -0.785674f}, // 电机2: 对角电机2
    {0.000000f, 0.000000f, 0.785674f},  // 电机3: 对角电机3
    {0.000000f, 0.000000f, -0.785674f}, // 电机4: 对角电机4

    // 轴向电机 (贡献Fx,Fy)
    {0.500000f, 0.000000f, 0.000000f},  // 电机5: X+轴向电机
    {-0.500000f, 0.000000f, 0.000000f}, // 电机6: X-轴向电机
    {0.000000f, 0.500000f, 0.000000f},  // 电机7: Y+轴向电机
    {0.000000f, -0.500000f, 0.000000f}  // 电机8: Y-轴向电机
};


void eight_thrust_allocate(const ControlCommand* v_d, float u[8])
{
	const float max_thrust =  10.0f;
	float u_offset[8] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};

    if (v_d == NULL || u == NULL ) {
        return;  // 参数检查
    }

    float u_theoretical[8] = {0};

    // ==================== 步骤1: 计算理论推力 u_theoretical = B_pinv * v_d ====================
    for (int i = 0; i < 8; i++) {
        u_theoretical[i] = 0.0f;
        for (int j = 0; j < 3; j++) {
            // 矩阵乘法: 伪逆矩阵(8x3) × 控制指令(3x1)
            u_theoretical[i] += B_pinv[i][j] * ((j == 0) ? v_d->Fx :
                                              (j == 1) ? v_d->Fy : v_d->Mz);
        }
    }

    // ==================== 步骤2: 处理负值约束（确保所有推力非负） ====================
    // 找出每组的最小值
    float min_diagonal = u_theoretical[0];   // 对角电机(0-3)的最小值
    float min_axial_x = u_theoretical[4];    // X轴向电机(4-5)的最小值
    float min_axial_y = u_theoretical[6];    // Y轴向电机(6-7)的最小值

    // 对角电机的最小值 (电机0-3)
    for (int i = 1; i < 4; i++) {
        if (u_theoretical[i] < min_diagonal) {
            min_diagonal = u_theoretical[i];
        }
    }

    // X轴向电机的最小值 (电机4-5)
    for (int i = 5; i < 6; i++) {
        if (u_theoretical[i] < min_axial_x) {
            min_axial_x = u_theoretical[i];
        }
    }

    // Y轴向电机的最小值 (电机6-7)
    for (int i = 7; i < 8; i++) {
        if (u_theoretical[i] < min_axial_y) {
            min_axial_y = u_theoretical[i];
        }
    }

    // 计算偏移量以确保每个推力非负
    float offset_diagonal = (min_diagonal < 0) ? -min_diagonal : 0.0f;
    float offset_axial_x = (min_axial_x < 0) ? -min_axial_x : 0.0f;
    float offset_axial_y = (min_axial_y < 0) ? -min_axial_y : 0.0f;

    // 应用方向偏移
    for (int i = 0; i < 8; i++) {
        u_offset[i] = u_theoretical[i];

        if (i < 4) {
            u_offset[i] += offset_diagonal;  // 对角电机偏移
        } else if (i < 6) {
            u_offset[i] += offset_axial_x;  // X轴向电机偏移
        } else {
            u_offset[i] += offset_axial_y;  // Y轴向电机偏移
        }
    }


	// ==================== 步骤3: 饱和限幅处理 ====================

	//先对对角4个电机进行饱和限幅（需要先饱和限幅）
	for (int i = 0; i < 4; i++)
	{
		// 限制在 [0, max_thrust] 范围内
		if (u_offset[i] < 0.0f)
		{
			u[i] = 0.0f;
		}
		else if (u_offset[i] > max_thrust)
		{
			u[i] = max_thrust;
		}
		else
		{
			u[i] = u_offset[i];
		}
  }

	float scaling_factor =  1.0f;
	int needs_scaling = 0;

	//检查是否需要缩放
	for(int i=4;i<8;i++)
	{
		if(u_offset[i]>max_thrust)
		{
			needs_scaling = 1;
			break;
		}
	}

	if(needs_scaling)
	{
		//计算缩放因子，找到最小缩放比
		float min_ratio = 1.0f;
		for(int i=4;i<8;i++)
		{
			float ratio = max_thrust/u_offset[i];
			if(ratio<min_ratio)
			{
				min_ratio = ratio;
			}
		}
		scaling_factor = min_ratio;

		//应用缩放
		for(int i=4;i<8;i++)
		{
			u[i] = u_offset[i]*scaling_factor;
		}
	}
	else
	{
		for(int i=4;i<8;i++)
		{
			u[i] = u_offset[i];
		}
	}


}
